#!/usr/bin/env python3
# filepath: /root/catkin_ws/src/teleop_twist_keyboard/teleop_joy.py

import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy


class JoystickControl:
    def __init__(self):
        rospy.init_node("joystick_control")

        # 订阅手柄的话题
        rospy.Subscriber("joy", Joy, self.joy_callback)

        # 创建发布者
        self.pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)

        # 配置参数
        self.linear_scale = rospy.get_param("~linear_scale", 0.5)  # 线性速度比例
        self.angular_scale = rospy.get_param("~angular_scale", 1.0)  # 角速度比例

        # 配置按钮映射
        self.axis_linear_x = rospy.get_param("~axis_linear_x", 1)  # 通常是左摇杆上下轴
        self.axis_angular_z = rospy.get_param(
            "~axis_angular_z", 0
        )  # 通常是左摇杆左右轴
        self.axis_linear_y = rospy.get_param(
            "~axis_linear_y", 3
        )  # 通常是右摇杆左右轴（用于全向移动）

        # 加速/减速按钮
        self.button_speed_up = rospy.get_param("~button_speed_up", 5)  # 通常是RB
        self.button_slow_down = rospy.get_param("~button_slow_down", 4)  # 通常是LB
        self.button_reset = rospy.get_param("~button_reset", 0)  # 通常是A键

        # 速度设置
        self.speed_multiplier = 1.0

        # 创建Twist消息实例
        self.twist = Twist()

        rospy.loginfo("手柄控制已启动！")
        rospy.loginfo("左摇杆: 移动控制")
        rospy.loginfo("RB/LB: 增加/减少速度")
        rospy.loginfo("A键: 重置速度")

    def joy_callback(self, joy_msg):
        # 按钮检测 - 速度调节
        if joy_msg.buttons[self.button_speed_up]:
            self.speed_multiplier = min(2.0, self.speed_multiplier + 0.1)
            rospy.loginfo(f"速度增加至: {self.speed_multiplier:.1f}x")

        if joy_msg.buttons[self.button_slow_down]:
            self.speed_multiplier = max(0.1, self.speed_multiplier - 0.1)
            rospy.loginfo(f"速度降低至: {self.speed_multiplier:.1f}x")

        if joy_msg.buttons[self.button_reset]:
            self.speed_multiplier = 1.0
            rospy.loginfo("速度重置为默认值")

        # 计算线速度和角速度
        self.twist.linear.x = (
            joy_msg.axes[self.axis_linear_x] * self.linear_scale * self.speed_multiplier
        )
        self.twist.angular.z = (
            joy_msg.axes[self.axis_angular_z]
            * self.angular_scale
            * self.speed_multiplier
        )

        # 对于全向机器人，设置Y轴速度
        self.twist.linear.y = (
            joy_msg.axes[self.axis_linear_y] * self.linear_scale * self.speed_multiplier
        )

        # 发布消息
        self.pub.publish(self.twist)


if __name__ == "__main__":
    try:
        controller = JoystickControl()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
    finally:
        # 确保机器人停止
        twist = Twist()
        controller.pub.publish(twist)
        rospy.loginfo("手柄控制已停止")
